/**********************************************************************************************
 * Arduino Delta Kinematics- Version 1.0
 * by William Bailes <williambailes@gmail.com> http://tinkersprojects.com/
 *
 * This Library is licensed under a GPLv3 License
 * 
 * made with infomation from the following documents
 * - http://forums.trossenrobotics.com/tutorials/introduction-129/delta-robot-kinematics-3276/
 * - https://www.marginallyclever.com/other/samples/fk-ik-test.html
 * - https://github.com/Tomdf/Delta_Robots/blob/master/Diagrams/Delta%20Robot%20Kinematics%20-%20Trossen%20Robotics.pdf
 * - http://hypertriangle.com/~alex/delta-robot-tutorial/
 **********************************************************************************************/

#include "DeltaKinematicsKe.h"

//#if ARDUINO >= 100
//  #include "Arduino.h"
//#else
//  #include "WProgram.h"
//#endif
//#include <QDebug>
#include <math.h>

/******************* SETUP *******************/

DeltaKinematicsKe::DeltaKinematicsKe(double _ArmLength_short, double armBiasIn,
                         double _RodLength, double topCircleIn,
                                     double buttomCircleIn,double zBiasIn)
{
  buttomTriangleSideLength = 2*tan(60.0/180.0*M_PI)*buttomCircleIn;
  topTriangleSideLength = 2*tan(60.0/180.0*M_PI)*topCircleIn;
  RodLength = _RodLength;//0.7
//  ArmLength=0.303;
  ArmLength = sqrt(_ArmLength_short*_ArmLength_short+armBiasIn*armBiasIn);//0.303
  _zeroBiasAngle=180.0*atan2(armBiasIn,_ArmLength_short)/M_PI;//8.156
  _zPositionBias=zBiasIn;//0
}

int DeltaKinematicsKe::forward_bias(double thetaA, double thetaB, double thetaC,
                                    double &xOut, double &yOut, double &zOut)
{
    thetaA+=_zeroBiasAngle;
    thetaB+=_zeroBiasAngle;
    thetaC+=_zeroBiasAngle;

    double tmpX,tmpY,tmpZ;
    int tmpKey= forward(thetaA,  thetaB,  thetaC, tmpX,tmpY,tmpZ);

    xOut=tmpY;//和keba一致
    yOut=-tmpX;//和keba一致
    zOut=-(tmpZ-_zPositionBias);//和keba一致
    return tmpKey;
}

int DeltaKinematicsKe::inverse_bias(double x0, double y0, double z0,
                                    double &aOut, double &bOut, double &cOut)
{
    double tmpX,tmpY,tmpZ;
    tmpY=x0;//和keba一致
    tmpX=-y0;//和keba一致
    tmpZ=_zPositionBias-z0;//和keba一致

    int tmpKey= inverse(tmpX,tmpY,tmpZ,aOut, bOut, cOut);
    aOut-=_zeroBiasAngle;
    bOut-=_zeroBiasAngle;
    cOut-=_zeroBiasAngle;


    return tmpKey;
}











// forward kinematics: (thetaA, thetaB, thetaC) -> (x0, y0, z0)
//int DeltaKinematicsKe::forward()
//{
//  return forward(a, b, c);
//}

int DeltaKinematicsKe::forward(double thetaA, double thetaB, double thetaC,
                               double &xOut,double &yOut,double &zOut)
{
  xOut=0.0;
  yOut=0.0;
  zOut=0.0;
  
  double t = (topTriangleSideLength-buttomTriangleSideLength)*tan30/2.0;
  double dtr = pi/180.0;

  thetaA *= dtr;
  thetaB *= dtr;
  thetaC *= dtr;

  double y1 = -(t + ArmLength*cos(thetaA));
  double z1 = -ArmLength*sin(thetaA);

  double y2 = (t + ArmLength*cos(thetaB))*sin30;
  double x2 = y2*tan60;
  double z2 = -ArmLength*sin(thetaB);

  double y3 = (t + ArmLength*cos(thetaC))*sin30;
  double x3 = -y3*tan60;
  double z3 = -ArmLength*sin(thetaC);

  double dnm = (y2-y1)*x3-(y3-y1)*x2;

  double w1 = y1*y1 + z1*z1;
  double w2 = x2*x2 + y2*y2 + z2*z2;
  double w3 = x3*x3 + y3*y3 + z3*z3;

  // x = (a1*z + b1)/dnm
  double a1 = (z2-z1)*(y3-y1)-(z3-z1)*(y2-y1);
  double b1 = -((w2-w1)*(y3-y1)-(w3-w1)*(y2-y1))/2.0;

  // y = (a2*z + b2)/dnm;
  double a2 = -(z2-z1)*x3+(z3-z1)*x2;
  double b2 = ((w2-w1)*x3 - (w3-w1)*x2)/2.0;

  // a*z^2 + b*z + c = 0
  double aV = a1*a1 + a2*a2 + dnm*dnm;
  double bV = 2.0*(a1*b1 + a2*(b2-y1*dnm) - z1*dnm*dnm);
  double cV = (b2-y1*dnm)*(b2-y1*dnm) + b1*b1 + dnm*dnm*(z1*z1 - RodLength*RodLength);

  // discriminant
  double dV = bV*bV - 4.0*aV*cV;
  if (dV < 0.0)
  {
    return non_existing_povar_error; // non-existing povar. return error,x,y,z
  }
  
  zOut = -0.5*(bV+sqrt(dV))/aV;
  xOut = (a1*zOut + b1)/dnm;
  yOut= (a2*zOut + b2)/dnm;

  return no_error;
}












// inverse kinematics
// helper functions, calculates angle thetaA (for YZ-pane)
int DeltaKinematicsKe::delta_calcAngleYZ(double *Angle, double x0, double y0, double z0)
{
  double y1 = -0.5 * 0.57735 * topTriangleSideLength;  // f/2 * tan(30 deg)
      y0 -= 0.5 * 0.57735 * buttomTriangleSideLength;  // shift center to edge

  // z = a + b*y
  double aV = (x0*x0 + y0*y0 + z0*z0 +ArmLength*ArmLength - RodLength*RodLength - y1*y1)/(2.0*z0);
  double bV = (y1-y0)/z0;

  // discriminant
  double dV = -(aV+bV*y1)*(aV+bV*y1)+ArmLength*(bV*bV*ArmLength+ArmLength); 
  if (dV < 0)
  {
//    qDebug()<<"non_existing_povar_error";
    return non_existing_povar_error; // non-existing povar.  return error, theta
  }

  double yj = (y1 - aV*bV - sqrt(dV))/(bV*bV + 1); // choosing outer povar
  double zj = aV + bV*yj;
  *Angle = atan2(-zj,(y1 - yj)) * 180.0/pi + ((yj>y1)?180.0:0.0);

  return no_error;  // return error, theta
}











// inverse kinematics: (x0, y0, z0) -> (thetaA, thetaB, thetaC)
//int DeltaKinematicsKe::inverse()
//{
//  inverse(x, y, z);
//}

int DeltaKinematicsKe::inverse(double x0, double y0, double z0,
                               double &aOut, double &bOut, double &cOut)
{
  aOut = 0;
  bOut = 0;
  cOut = 0;
  int error = delta_calcAngleYZ(&aOut, x0, y0, z0);
  if(error != no_error)
  {
      return error;
  }

  error = delta_calcAngleYZ(&bOut, x0*cos120 + y0*sin120, y0*cos120-x0*sin120, z0);
  if(error != no_error)
  {
      return error;
  }

  error = delta_calcAngleYZ(&cOut, x0*cos120 - y0*sin120, y0*cos120+x0*sin120, z0);

  return error;
}
